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ABSTRACT  |  This  paper  describes  a  novel  representation  for 
two-dimensional  maps,  and  shows  how  this  representation 
may  be  applied  to  the  problem  of  multirobot  simultaneous 
localization  and  mapping.  We  are  inspired  by  the  notion  of  a 
manifold,  which  takes  maps  out  of  the  two-dimensional  plane 
and  onto  a  surface  embedded  in  a  higher-dimensional  space. 
The  key  advantage  of  the  manifold  representation  is  self- 
consistency:  when  closing  loops,  manifold  maps  do  not  suffer 
from  the  “cross  over”  problem  exhibited  in  planar  maps.  This 
self-consistency,  in  turn,  facilitates  a  number  of  important 
capabilities,  including  autonomous  exploration,  search,  and 
retro-traverse.  It  also  supports  a  very  robust  form  of  loop 
closure,  in  which  pairs  of  robots  act  collectively  to  confirm  or 
reject  possible  correspondence  points.  In  this  paper,  we  de¬ 
velop  the  basic  formalism  of  the  manifold  representation,  show 
how  this  may  be  applied  to  the  multirobot  simultaneous 
localization  and  mapping  problem,  and  present  experimental 
results  obtained  from  teams  of  up  to  four  robots  in  environ¬ 
ments  ranging  in  size  from  400  to  900  m2. 
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I.  INTRODUCTION 

Many  mobile  robot  tasks,  such  as  exploration  and  search, 
are  performed  in  environments  that  are  only  partially 
mapped.  As  a  result,  these  autonomous  behaviors  must  be 
performed  concurrently  with  a  simultaneous  localization 
and  mapping  process  (SLAM).  In  this  paper,  we  develop  a 
map  representation  that  remains  self-consistent  with 
respect  to  common  navigation  problems  (such  as  path 
planning),  while  also  supporting  the  simultaneous  local¬ 
ization  and  mapping  process.  Standard  planar  maps  are  ill- 
suited  for  this  purpose,  due  their  tendency  to  become 
confused  (at  least  temporarily)  in  environments  contain¬ 
ing  loops.  Consider,  for  example,  the  situation  shown  in 
Fig.  1:  as  the  robot  traverses  a  partial  loop,  the  path  of  the 
robot  crosses  over  itself.  While  this  inconsistency  may  be 
ultimately  be  resolved  by  moving  further  along  the  loop, 
the  planar  map  cannot  be  used  for  path-planning  in  the 
interim.  In  contrast,  under  the  same  conditions,  a  manifold 
representation  remains  entirely  self-consistent.  Robots  can 
construct  paths,  for  example,  as  long  as  those  paths  remain 
embedded  in  the  manifold.  Furthermore,  if  the  robot  does 
ultimately  close  the  loop,  the  manifold  can  be  collapsed  to 
recover  a  self-consistent  planar  representation. 

The  manifold  representation  facilitates  a  number  of 
interesting  capabilities.  For  example,  using  incremental 
mapping  alone  (i.e.,  no  loop  closures),  a  robot  can  always 
retro-traverse  to  any  previously  visited  location  (or,  more 
precisely,  to  any  point  on  the  manifold).  In  this  case,  the 
same  location  in  the  world  may  be  represented  more  than 
once  in  the  manifold:  if  the  robot  traverses  a  loop  in  one 
direction,  for  example,  the  manifold  will  develop  a  spiral 
structure,  with  the  same  locations  being  repeated  over  and 
over  again.  In  spite  of  this  ambiguity,  however,  the  robot 
can  always  retro-traverse  by  traveling  back  along  the  spiral 
structure. 
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The  many-to-one  relationship  between  points  on  the 
manifold  and  points  in  the  world  gives  rise  to  a  second 
interesting  capability:  lazy  loop  closure.  Loop  closure  is  the 
most  difficult  part  of  the  simultaneous  localization  and 
mapping  process:  in  order  to  close  a  loop,  one  must  decide 
that  two  points  in  the  map  correspond  to  the  same  point  in 
the  world  (this  is  the  data-association  problem).  In  the 
manifold  representation,  such  decisions  can  be  indefinitely 
delayed,  without  risking  map  consistency;  thus,  one  may 
wait  until  robots  acquire  more  information  to  conclusively 
establish  the  correspondence  (or  lack  thereof)  between 
two  points.  In  the  multirobot  context,  this  allows  us  to  take 
active  steps  to  discover  correspondence  points,  using  pairs 
of  robots  acting  in  concert.  Thus,  for  example,  a  pair  of 
robots  can  arrange  a  rendezvous  at  two  points  on  the 
manifold  that  may  or  may  not  represent  the  same  location 
in  the  world:  if  the  robots  meet,  the  points  match  and  the 
loop  is  closed;  if  they  fail  to  meet,  the  points  are  distinct 
and  there  is  no  loop. 

This  paper  makes  no  attempt  to  cover  all  aspects  the 
manifold  representation  outlined  above.  Instead,  we 
restrict  ourselves  to  introducing  the  basic  methodology 
and  applying  it  to  the  specific  problem  of  multirobot 
mapping.  We  take  maximum  likelihood  estimation  (MLE) 
techniques  that  have  previously  been  applied  to  simulta¬ 
neous  localization  and  mapping  [1],  [2],  and  adapt  those 
techniques  for  the  manifold  representation.  For  validation, 
we  present  experimental  mapping  results  from  a  number 
environments,  using  teams  of  up  to  four  robots,  under  both 
manual  and  autonomous  control. 


II.  RELATED  WORK 

Thrun’s  survey  paper  [3]  identifies  a  relatively  small  set  of 
probabilistic  methods  underlying  most  recent  SLAM 
algorithms:  these  include  Kalman  filters  [4],  [5],  expecta¬ 
tion  maximization  [6],  incremental  maximum  likelihood 
[2],  and  various  hybrid  methods  [7].  Recent  work  on 
FastSLAM  algorithms  (which  approximate  the  full  poste¬ 
rior  distribution  over  maps  using  a  particle  filter)  should 
be  added  to  this  list  [8],  [9].  The  approach  described  in  this 
paper  makes  use  of  both  incremental  MLE  (for  fine-scale 
localization)  and  Lu-and-Milios-style  global  map  align¬ 
ment  [1].  We  also  make  use  of  local  map  patches  to  enforce 
global/topological  map  consistency;  this  is  similar  in 
concept,  if  not  in  detail,  to  the  approach  taken  in  the 
Atlas  framework  [10],  and  in  hybrid  extensions  to  the 
Spatial  Semantic  Hierarchy  [11]. 

Existing  SLAM  methods  have  known  limitations,  the 
most  important  of  which  is  sensitivity  to  false  data 
associations.  The  manifold  representation  is  designed,  in 
part,  to  overcome  this  limitation:  it  provides  a  mechanism 
whereby  data-association  decisions  can  be  postponed  until 
such  time  as  they  can  be  made  with  high  confidence. 
Hahnel  [12]  describes  an  alternative — and  possibly  more 
powerful — approach  to  this  problem,  in  which  bad  data- 
association  decisions  may  be  retracted  by  stepping  back 
through  a  tree-structured  representation  of  possible  maps. 

It  should  be  noted  that  the  research  described  in  this 
paper  was  spurred  by  a  very  specific  programmatic 
challenge:  to  deploy  a  large  number  of  robots  into  an 
unexplored  building,  map  the  building  interior,  detect  and 
track  intruders,  and  transmit  all  of  the  above  information 
to  a  remote  operator.  The  multirobot  systems  built  to  meet 
this  challenge  are  described  in  [13]  and  [14]. 


III.  MAPPING  ON  A  MANIFOLD: 

CORE  CONCEPTS 

The  key  conceptual  difficulty  with  manifold  mapping  lies 
in  the  representation  of  the  manifold  itself.  In  principle, 
the  manifold  is  an  arbitrarily  complex  structure  with 
varying  local  curvature;  in  practice,  the  representation 
must  be  discrete,  and  hence  some  degree  of  approximation 
and  linearization  is  inevitable.  In  this  section,  we  develop 
the  basic  concepts,  definitions,  and  notation  used  in  our 
approximated  representation. 

A.  Patches 

The  manifold  is  discretized  by  dividing  it  into  a  set  of 
overlapping  patches,  each  of  which  has  finite  extent  and 
defines  a  local  (planar)  coordinate  system.  Let  n  denote 
the  set  of  such  patches;  we  make  the  following  definition: 


n  =  M:7r=(M  (i) 


Fig.  1.  Illustration  of  a  partially  closed  loop,  (a)  Planar  representation. 
( b )  Manifold  representation. 
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(a) 


(b) 


(c) 


Fig.  2.  Incremental  localization  and  mapping,  (a)  and  (b)  A  new  laser  scan  (dotted  polygon)  is  fitted  against  the  robot's  local  map  (solid 
polygon ),  to  generate  a  corrected  robot  pose  estimate,  (c)  If  the  laser  scan  covers  unexplored  regions  of  the  manifold ,  a  new  patch  is 
added  to  the  map. 


where  an  individual  patch  i r  consists  of  a  free-space  polygon 
s  describing  the  extent  of  the  patch,1  and  a  projected  planar 
pose  9  that  defines  the  patch-local  coordinate  system  ( 9  is 
obtained  by  projecting  the  origin  of  the  patch  onto  a 
canonical  plane). 

Given  these  definitions,  the  pose  of  any  object  on  the 
manifold  can  subsequently  be  described  by  a  tuple 
p  =  (7 r,  r)  specifying  a  particular  patch  ir  and  the  pose  r 
of  the  object  with  respect  to  that  patch  (r  must  lie  inside 
the  patch  polygon  s).  Importantly,  since  patches  may 
overlap,  the  tuple  p  need  not  be  unique:  one  can  also  write 
down  the  same  object’s  pose  as  p  =  (7 f,  r)  where  r  is  the 
pose  relative  to  some  overlapping  patch  7f.  Two  questions 
naturally  arise  from  this  apparent  ambiguity:  how  does  one 
establish  that  the  tuples  p  and  p  represent  the  same  pose 
on  the  manifold,  and  how  does  one  transform  a  pose 
specified  with  respect  to  patch  7 r  into  a  pose  specified  with 
respect  an  overlapping  patch  7f?  Consider  the  manifold 
illustrated  in  Fig.  2:  each  point  on  the  manifold  will 
project  onto  exactly  one  point  on  an  imaginary  horizontal 
plane,  and,  conversely,  some  points  on  the  plane  will 
project  onto  multiple  points  on  the  manifold.  This 
observation  leads  to  the  following  condition:  the  tuples  p 
and  p  represent  the  same  point  on  the  manifold  if  and  only 
if  the  projections  of  the  polygons  s  and  s  overlap,  and  the 
projections  of  r  and  r  are  identical.  Mathematically,  this 
condition  is  stated  as  follows: 

(s  ®  9)  fl  (s  ®  9)  7^  0  and  r®0-f®^=O  (2) 

where  ®  is  a  coordinate  transform  operator.  Given  a 
projected  pose  9  and  patch-relative  pose  r,  the  expres- 

1Strictly  speaking,  we  use  polysolids  rather  than  polygons  for 
representing  free  space,  since  polysolids  form  a  group  under  the  operations 
of  union  and  intersection  (polysolids  can  have  holes).  The  term  “polysolid” 
appears  to  have  been  coined  by  H.  Maynard  and  L.  Tavernini  at  the 
University  of  Texas  at  San  Antonio;  their  work  was  never  published,  but  is 
similar  in  concept  to  the  polygon  sets  described  in  [15]. 


sion  r  ®  9  yields  the  corresponding  projected  pose  q. 
One  can  also  define  the  inverse  operator  ©:  given  two 
projected  poses  9  and  q,  the  inverse  expression  q  ©  9 
recovers  the  patch-relative  pose  r.  These  operators  obey 
the  normal  rules  for  algebraic  associativity,  but  do  not 
commute. 

From  the  identity  expressed  in  (2),  one  can  trivially 
derive  the  coordinate  transform  equations  for  overlapping 
patches 


r  =  r  (&  9  Q  9  and  r  =  r  (&  9  Q  9.  (3) 


Collectively,  (2)  and  (3)  provide  the  necessary  tools  for 
working  with  manifold  poses  and  their  planar  projections. 
Importantly,  one  can  use  these  equations  to  construct 
paths  on  the  manifold. 

B.  Relations 

For  concurrent  localization  and  mapping,  the  projected 
poses  of  the  patches  Ft  are  not  known  a  priori;  instead  we 
have  a  set  of  relations  that  constrain  the  patches’  relative 
pose  (a  scan-matching  algorithm,  for  example,  may 
establish  point-to-point  correspondences).  Let  denote 
the  set  of  pairwise  relations  between  patches;  we  write 


$  =  {(/>}:  0=  (7r,7r;x,x,  a)  (4) 


where  the  relation  (j)  implies  that  point  x  on  patch  7T 
corresponds  to  point  x  on  patch  7 f;  a  is  the  uncertainty 
associated  with  the  correspondence.  One  can  write  down 
similar  definitions  for  point-to-line,  line-to-line,  relative 
range,  and  relative  bearing  relations. 
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C.  Fitting  Patches 

Given  the  above  definitions,  one  can  apply  MLE 
techniques  to  find  the  set  of  projected  poses  0  =  {0} 
that  is  most  likely  to  generate  the  observed  set  of  relations 
=  {(/)}.  That  is,  MLE  searches  for  the  estimate  0  that 
maximizes  the  conditional  probability  P(T>|0) 

0  =  argmaxP(T>|0) 

=  argmaxP[p(^|0)  (5) 

0 


where  we  make  the  additional  assumption  that  the 
relations  in  <f>  represent  statistically  independent  observa¬ 
tions.  Applying  the  standard  log-likelihood  transformation 
to  these  equations,  one  can  equivalently  search  for  the  0 
that  minimizes  the  (negative)  conditional  log-likelihood 

'  (-i-  ©) 

@  =  argimny^L((/.|©).  (6) 

0 

This  latter  form  is  more  convenient  for  most  practical 
purposes.  For  point-to-point  relations  with  Gaussian 
uncertainty,  the  log-likelihood  for  a  single  relation  (f  is 
given  by 

L(0|0)  =  —r  (x  0  6  -  x  0  Q)2  (7) 

2(7z 


where  x  0  0  denotes  the  projected  pose  of  a  point  on  patch 
7 r  and  5c0  0  denotes  the  projected  pose  of  the 
corresponding  point  on  patch  7 f.  Intuitively,  one  can 
visualize  the  two  points  as  being  pulled  together  by  a 
simple  spring. 

In  principle,  the  maximum  likelihood  estimate  0  can 
be  found  by  solving 

O  =  ^VI(0|0).  (8) 

In  the  case  of  two-dimensional  mapping,  however,  the 
gradient  terms  on  the  right-hand  side  of  (8)  generally 
contain  transcendental  components,  and  hence  there 
exists  no  closed-form  solution.  Fortunately,  a  range  of 
numeric  optimization  techniques  can  used  to  find  the 
minimum,  including  simple  gradient  descent  and  its  more 


refined  brethren,  such  as  the  Levenburg-Marquardt  and 
Fletcher-Reeves  algorithms  [16]. 

The  confidence  in  the  estimate  0  can  be  determined 
by  inspecting  the  local  curvature  of  VL(T>|0)  around  0. 
Following  the  standard  practice  in  the  MLE  literature 
[17],  we  write  down  the  stochastic  Fisher  information 
matrix  as 


7($|0)=^V2L(</>|0).  (9) 

The  confidence  interval  a  *  on  any  component  i  of  0  is  then 
given  by 

=  c\j  (rH^I©))..  (io) 


where  c  is  an  appropriate  critical  value  (e.g.,  1.96  for  a  95% 
confidence  interval),  and  the  ii  subscript  denotes  a 
particular  component  of  the  inverted  Fisher  information 
matrix.  In  practice,  we  are  usually  less  interested  in  the 
absolute  fit  confidence  than  we  are  in  the  relative 
confidence  between  a  given  pair  of  patches;  i.e.,  how 
well  is  the  pose  of  patch  7 r  determined  with  respect  to 
patch  7f?  This  quantity  can  be  determined  using  a  sightly 
modified  version  of  the  above  procedure:  we  treat  the 
components  of  0  that  correspond  to  patch  it  as  constants, 
and  eliminate  the  corresponding  rows  and  columns  from 
the  Fisher  information  matrix.  Inserting  this  reduced 
matrix  into  (10),  we  obtain  the  component-wise  confi¬ 
dence  intervals  for  every  patch,  relative  to  the  “fixed” 
patch  it.  For  the  remainder  of  this  paper,  we  will  use 
J(T>|0)  to  denote  this  reduced  matrix. 

IV.  MULTIROBOT  MAPPING 

We  turn  now  to  the  specific  problem  of  multirobot 
mapping,  using  the  mathematical  tools  described  in  the 
previous  section.  This  problem  can  be  broken  into  three 
subproblems:  incremental  localization  and  mapping,  loop 
closure,  and  island  merging. 

Incremental  localization  and  mapping  is  the  basic 
mode  of  operation  for  the  mapping  algorithm:  as  each 
robot  moves  through  the  environment,  odometry  and  laser 
data  are  used  to  update  the  robot’s  current  pose  estimate 
and,  under  certain  circumstances,  to  make  incremental 
additions  to  the  map.  The  basic  process  is  illustrated  in 
Fig.  2  and  described  in  detail  in  the  next  section.  Note  that 
robots  extend  the  map  at  the  edges  of  the  manifold  only;  a 
robot  that  is  retro-traversing  to  a  previously  visited 
location  will  not  add  to  the  map. 
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(a) 


(b) 


(c) 


Fig.  3.  A  loop  closure  triggered  by  a  mutual  observation,  (a)  &  (b)  Two  robots  observe  each  other ,  generating  a  new  relation,  (c)  The  change 
in  topology  is  propagated  through  the  manifold,  inducing  new  relations  and  forcing  patches  to  be  refitted. 


This  process  is  punctuated  by  two  events  that  require 
global  changes  to  the  map:  loop  closure  and  island 
merging.  Loop  closure  is  the  process  whereby  two  widely 
separated  regions  of  the  map  are  brought  together  (see 
Fig.  3).  In  Section  III-A,  we  showed  that  multiple  points 
on  the  manifold  may  project  onto  the  same  point  on  the 
plane;  in  the  context  of  mapping,  this  implies  that  two 
widely  separated  points  on  the  manifold  may  in  fact 
represent  the  same  location  in  the  world.  Indeed,  if  one 
uses  incremental  mapping  alone,  any  loops  in  the 
environment  will  be  “unrolled”  to  form  spiral  struc¬ 
tures,  with  the  same  series  of  locations  repeating  over 
and  over  again  in  the  manifold.  Loop  closure,  then,  is 
the  process  whereby  such  repeated  locations  are 
identified,  and  the  topology  of  the  manifold  is  modified 
accordingly. 

In  a  similar  vein,  island  merging  is  the  process  whereby 
two  unconnected  regions  of  the  manifold  are  combined 
into  a  single  representation  (see  Fig.  4).  In  the  context  of 
multirobot  mapping,  there  are  two  basic  scenarios  that 
give  rise  to  such  islands:  robots  enter  the  environment 
from  separate  locations,  or  robots  enter  the  environment 
from  the  same  location,  but  at  different  times.  In  either 


case,  we  proceed  by  building  a  separate  island  for  each 
robot,  and  merging  those  islands  only  when  a  suitable 
correspondence  point  has  been  established. 

The  loop-closure  and  island-merging  processes  depend 
on  our  ability  to  uniquely  identify  a  particular  location  in 
the  world  (the  traditional  data-association  problem).  In  the 
case  of  single-robot  mapping,  there  are  two  basic  methods 
for  making  this  identification:  recognizing  a  unique 
feature  associated  with  that  location  (including  preplaced 
fiducials)  or  making  plausible  guesses  based  on  patterns  of 
nonunique  features.  These  two  methods  have  been  well 
treated  in  the  single-robot  mapping  literature  [2],  [6],  [7], 
[12],  and  will  not  be  covered  here.  Instead,  we  focus  on  a 
third  method  that  is  unique  to  the  multirobot  mapping 
domain:  using  the  robots  themselves  as  unambiguous 
landmarks.  Whenever  two  robots  sight  one  another — a 
process  we  refer  to  as  mutual  observation — we  establish  a 
correspondence  between  two  points  on  the  manifold; 
mutual  observations  can  therefore  be  used  to  close  loops 
and  merge  islands. 

In  the  following  sections,  we  describe  the  incremental 
mapping,  loop-closure,  and  island-merging  processes  in 
detail.  Note  that,  throughout  this  presentation,  we  assume 


(a) 


(tO 


(c) 


Fig.  4.  Merging  islands  after  a  mutual  observation,  (a)  Two  robots  observe  each  other,  generating  a  new  relation,  (b)  &  (c)  The  two  islands 
are  roughly  aligned,  and  the  change  in  topology  is  propagated  through  the  manifold. 
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that  the  mapping  algorithm  is  centralized;  i.e.,  data  from  all 
of  the  robots  is  communicated  to  a  common  location, 
where  it  is  assembled  to  form  a  map. 

A.  Incremental  Localization  and  Mapping 

Incremental  localization  and  mapping  is  performed 
independently  and  concurrently  for  each  robot  on  the 
team.  Two  pieces  of  sensor  data  are  used  in  this 
process:  odometry  data  (which  measures  changes  in  the 
robot’s  pose)  and  laser  scan  data  (which  measures  the 
range  and  bearing  of  nearby  features). 

Let  ottf  be  the  measured  (odometric)  change  in  pose 
between  times  t  and  t',  and  let  st'  be  the  laser  scan  that  is 
subsequently  recorded.  If  pt  =  (7 rt,rt)  is  the  robot  pose 
estimate  at  some  time  t,  the  updated  robot  pose  estimate 
Pt'  —  (tt t',rt')  can  be  determined  as  follows. 

1)  Create  a  new  patch  7 r*  =  (#*,s*)  such  that 

9*  0^  ®  rt  ®  6t  and  s*  st/,  (11) 

i.e.,  the  projected  pose  0 *  is  computed  by 
combining  the  measured  (odometric)  change  in 
pose  ottf  with  the  robot’s  current  pose  estimate  pt. 

2)  Create  a  local  map  around  the  current  patch  irt; 
the  local  map  is  the  set  of  patches  II*  that  are  both 
nearby  (in  the  planar  projection)  and  well  fitted 
with  respect  to  7Tt;  that  is,  II*  contains  all  patches 
7T  that  satisfy  the  condition 


1.96^(j-i(*|0))w<e.  (12) 

The  notation  here  requires  a  little  explanation: 
J(T>|@)  is  the  Fisher  information  matrix  computed 
for  patch  7 rt  (see  Section  III-C);  the  patch  7T  is 
included  in  the  local  map  only  if  the  confidence 
interval  on  every  component  of  9  is  less  than  some 
threshold  e. 

3)  Match  features  in  the  new  patch  7 r*  against 
features  in  the  local  map  Ft*.  We  omit  the  details 
of  the  scan-matching  algorithm  and  assume  only 
that  it  produces  some  set  of  relations  <f>*. 

4)  Use  MLE  to  fit  the  new  patch  against  the  local 
map;  i.e.,  find  the  projected  pose  6 *  that  is  most 
likely  to  give  rise  to  the  observed  relations  T>* 

#*  arg  min  !(<!>*  | #).  (13) 

9 

The  minimum  value  is  found  using  numeric 
optimization. 


5)  Compute  the  new  robot  pose  estimate 
Pt'  =  (7 rt',rt/)  by  projecting  the  new  patch  back 
into  the  manifold 

7iy  argmin  ||0*  ©  9\\ 

Trent 

77  <-0*0  0,  (14) 

where  nt  is  a  subset  of  II*  containing  only 
those  patches  whose  scan  polygons  overlap  with 
7Tt;  the  potential  ambiguity  in  the  projection  is 
resolved  by  selecting  the  nearest  patch  from 
this  set. 

Steps  3  and  4  of  the  algorithm  may  be  applied  iteratively 
(EM  style)  to  improve  the  quality  of  the  fit. 

The  key  step  in  this  algorithm  lies  in  the  creation  of  the 
local  map.  In  effect,  that  part  of  the  manifold  that  is  well 
localized  with  respect  to  the  robot  is  projected  onto  a 
plane;  the  robot  is  then  localized  by  fitting  its  laser  scan 
against  this  planar  projection.  In  this  context,  the  choice  of 
the  threshold  e  becomes  crucial,  since  this  parameter 
implicitly  controls  the  number  of  patches  included  in  the 
local  map:  if  e  is  too  small,  there  may  not  be  enough 
patches  to  adequately  constrain  the  robot  pose;  if  e  is  too 
large,  the  local  map  may  contain  gross  inconsistencies  that 
lead  to  widely  inaccurate  pose  estimates. 

Having  localized  the  robot,  we  may  need  to  extend  the 
map.  There  are  a  number  of  conditions  that  can  trigger  this 
process:  e.g.,  the  new  patch  is  far  from  any  of  the  existing 
patches  in  the  local  map,  or  the  new  patch  covers’  a 
significant  area  of  the  manifold  that  is  not  covered  by  the 
current  local  map  n*.  If  none  of  these  conditions  are  true, 
the  patch  7T*  is  discarded;  otherwise,  the  patch  and  its 
relations  are  appended  to  the  map.  In  this  case,  the  robot 
pose  estimate  /ty  =  (717,77)  must  also  reset  such  that  the 
robot  lies  at  the  origin  of  the  new  patch;  i.e., 

7iy  7T*  and  rt>  <—  0.  (15) 

B.  Loop  Closure 

The  loop-closure  algorithm  is  triggered  by  events 
(such  as  mutual  observation)  that  generate  new  relation¬ 
ships  between  previously  unrelated  patches.  The  key 
challenge  for  this  algorithm  lies  not  in  the  integration  of 
this  singular  relation  into  the  map;  rather,  it  lies  in  the 
integration  of  any  additional  relations  that  may  be  induced 
as  the  change  is  propagated  through  the  map.  Consider, 
for  example,  a  pair  of  robots  traveling  in  opposite  direc¬ 
tions  around  a  circular  environment.  If  the  robots  should 
fail  to  observe  each  other  on  the  first  few  passes  (and  thus 
fail  to  close  the  loop),  the  manifold  will  develop  a  double 
spiral  structure  (one  spiral  for  each  robot).  The  loop- 
closure  algorithm  must  be  such  that  a  single  subsequent 
mutual  observation  will  the  collapse  the  entirety  of  both 
spirals  into  a  single  loop. 
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Our  method  for  achieving  this  collapse  is  as  follows: 
given  a  new  relation,  the  algorithm  propagates  changes 
outwards  from  the  closure  point,  alternating  between 
inducing  new  relations  and  refitting  the  map.  The  process 
terminates  only  when  no  new  relations  can  be  induced.  Let 
(j)ai,  denote  a  new  relation  between  previously  unrelated 
patches  7Ta  and  7 r^;  the  algorithm  is  as  follows. 

1)  Add  the  new  relation  (pai,  to  the  map  and  refit 

©  argmmy^L(0|0').  (16) 

0 

2)  Create  a  queue  of  patches;  initialize  the  queue 
with  the  set  of  all  patches  that  are  related  to  7Ta 
or  7Tb. 

3)  Pop  the  first  patch  off  the  queue;  call  this  patch 
7r* .  Induce  the  local  map  II*  for  this  patch  using 
the  procedure  described  in  Section  IV-A. 

4)  Use  scan-matching  to  fit  the  patch  7 r*  to  the  local 
map  II*.  From  the  set  of  relations  T>*  found  by  the 
scan-matching  algorithm,  eliminate  those  that  are 
already  in  the  map;  i.e.,  T>*  should  represent  the 
set  of  new  relations  for  patch  tt. 

5)  If  T>*  is  nonempty: 

a)  add  the  new  relations  <f>*  to  the  map  and  refit 
as  per  step  1; 

b)  add  all  patches  that  are  related  to  7 r*  to  the 
queue. 

The  process  continues  until  the  queue  is  empty. 

Compared  to  incremental  localization  and  mapping, 
the  loop-closure  algorithm  is  relatively  expensive:  refitting 
the  entire  map  is  nontrivial,  and  may  be  performed  more 
than  once  for  any  given  loop  closure.  Fortunately,  closure 
events  are  relatively  rare  (their  frequency  depends  on  the 
number  of  loops  in  the  environment).  In  addition,  the 
loop-closure  algorithm  is  executed  only  once  for  each  loop 


in  the  environment;  subsequent  traversals  of  a  loop  will 
incur  no  penalty. 

C.  Island  Merging 

The  island-merging  algorithm  is  triggered  by  events 
that  induce  relations  between  patches  belonging  to  sep¬ 
arate  islands  (patches  are  said  to  belong  to  the  same 
island  only  if  and  only  if  they  are  connected  by  some 
sequence  of  relations).  In  this  case,  as  with  loop  closure, 
we  must  admit  the  possibility  that  there  is  substantial 
overlap  between  the  two  islands,  and  any  changes  made 
at  the  point  where  the  islands  are  merged  must  be  pro¬ 
pagated  throughout  the  map.  The  algorithm  for  island 
merging  is  therefore  identical  to  that  used  for  loop 
closure,  with  one  exception:  prior  to  merging,  we  treat 
the  two  islands  as  rigid  bodies  and  quickly  bring  them 
into  rough  alignment,  thus  saving  a  great  deal  of  time  in 
the  refitting  process. 

Compared  with  incremental  mapping,  island  merging 
is  a  relatively  expensive  process.  For  a  team  of  N  robots, 
however,  the  algorithm  will  be  executed  at  most  N  —  1 
times;  moreover,  since  robots  are  likely  to  commence 
mapping  from  a  relatively  small  number  of  initial 
locations,  most  of  these  mergers  are  trivial  (i.e.,  involving 
islands  with  only  a  handful  of  patches). 

V.  EXPERIMENTS 

The  multirobot  mapping  approach  described  in  this  paper 
has  been  applied  to  a  wide  range  of  environments  of 
varying  size  and  complexity.  Fig.  5  shows  a  selection  of  the 
final  occupancy  grid  maps  produced  by  the  algorithm; 
these  maps  were  generated  autonomously,  and  in  real  time 
(the  data  sets  used  to  generate  these  maps  can  be 
downloaded  from  the  Radish  [18]  Web  site). 

Fig.  6  shows  the  result  for  one  particularly  challenging 
experiment  conducted  under  external  supervision  (i.e.,  the 
environment  and  test  conditions  were  selected  by  an 


Fig.  5.  Occupancy  grids  generated  by  the  mapping  package,  (a)  SAIC  site  A  (two  robots),  (b)  SAIC  site  B  (four  robots),  (c)  USC  Science  Library 
(four  robots). 
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Fig.  6.  (a)  A  four-robot  mapping  team ;  each  robot  carries  a  unique  laser-visual  bar  code  to  facilitate  mutual  recognition,  (b)  Final  occupancy 
grid  produced  during  a  multiple-robot,  multiple-entry  trial:  two  robots  enter  from  the  door  at  the  right  top,  another  two  robots  enter 
from  the  door  at  the  right  bottom  (the  relative  pose  of  the  two  doorways  is  unknown).  The  environment  is  approximately  600  m2  in  area. 


independent  group).  Four  robots  were  deployed  into  the 
test  environment  and  executed  an  autonomous  exploration 
and  search  algorithm.  Importantly,  the  robots  were 
deployed  from  two  different  entry  points,  and  the  relative 
pose  of  these  points  was  unknown.  Each  pair  of  robots  was 
therefore  required  to  explore  and  map  independently, 
until  such  time  as  the  teams  came  within  line  of  sight  of 


one  another.  To  facilitate  mutual  observation,  each  robot 
was  equipped  with  a  scanning  laser-range  finder,  a  camera, 
and  a  laser-visual  bar  code.  Using  the  bar  code,  each  robot 
can  determine  the  identity,  range  and  bearing  of  other 
robots;  through  the  exchange  of  such  observations,  each 
pair  of  robots  can  determine  their  relative  pose  to  within  a 
few  centimeters  (over  a  distance  of  about  8  m). 


id)  (e)  (f) 


Fig.  7.  (a)  Occupancy  grid  generated  by  the  pair  of  robots  entering  the  environment  from  the  top  right.  Open  circles  show  the  current  robot 
locations ;  closed  circles  show  the  patch  centers,  (b)  Occupancy  grid  generated  by  the  pair  of  robots  enteringfrom  the  bottom  left.  Note  that  these 
maps  have  been  reoriented  to  facilitate  visual  comparison ;  from  the  robots  perspective,  the  relative  pose  of  the  two  maps  is  unknown. 

(c)  A  mutual  observation  (dotted  line)  allows  the  maps  to  be  roughly  aligned,  (d)  and  (e)  Relationships  are  incrementally  propagated 
through  the  manifold,  (f)  Relaxed  grid  map  using  data  from  all  four  robots. 
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Fig.  7  illustrates  the  evolution  of  the  map  over  the 
course  of  the  experiment.  Initially,  the  two  teams  con¬ 
struct  separate  maps  (or  islands)  as  shown  in  Fig.  7(a)  and 
(b).  After  a  few  minutes  of  autonomous  exploration, 
however,  one  of  the  robots  from  the  first  team  encounters 
a  robot  from  the  second  (the  two  robots  make  a  mutual 
observation  and  determine  their  relative  pose).  This 
triggers  an  island-merging  event  (Section  IV-C),  which 
first  brings  the  two  maps  into  rough  alignment  [Fig.  7(c)], 
then  iteratively  propagates  new  relationships  between  the 
maps  [Fig.  7(d)  and  (e)].  The  final  combined  map  after 
island  merging  is  shown  in  Fig.  7(f).  Exploration  continues 
with  all  four  robots,  ultimately  producing  the  map  shown 
in  Fig.  6(b). 

It  should  be  noted  that  this  map  was  generated  online 
and  in  real  time  (i.e.,  the  time  required  to  produce  the 
map  was  less  than  the  time  required  for  the  robots  to 
explore  the  environment).  During  island  merging  or  loop 
closure,  however,  it  is  necessary  to  halt  the  robots  for 
some  tens  of  seconds  to  allow  the  mapping  process  to 
“catch  up.” 


VI.  CONCLUSION  AND  FURTHER  WORK 

While  the  manifold  techniques  described  in  this  paper 
may  be  used  to  generate  maps  from  robots  under  manual 
control,  our  key  motivation  in  designing  this  represen¬ 
tation  is  to  support  autonomous  behaviors  for  incom¬ 
pletely  mapped  environments.  As  a  result,  our  current 
research  is  heavily  focused  on  this  topic.  To  date,  we 
have  created  an  autonomous  exploration  algorithm  that 
exploits  the  manifold  map  to  direct  and  coordinate  mul¬ 
tiple  robots;  our  near-term  aim  is  to  extend  this  algo¬ 
rithm  to  include  planned  rendezvous  [19]:  i.e.,  the  use  of 
robots  in  pairs  to  explore  regions  of  the  manifold  that 
appear  similar. 

The  algorithm,  as  presented,  also  has  some  key 
limitations:  since  it  relies  on  centralized  processing,  it  is 
sensitive  to  communication  failures  and  scales  poorly  with 
increasing  team  size  (the  largest  experiment  conducted  to 
date  employed  just  four  robots).  Developing  a  truly 
distributed  version  of  this  algorithm  thus  remains  an 
open  challenge. 
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